/**
 * @file targetsolver.h
 * @brief PnP解算
 * @details 得到相机坐标系
 */
#pragma once
#ifndef TARGETSOLVER_HPP
#define TARGETSOLVER_HPP
#include <fstream>
#include <opencv2/opencv.hpp>


#include "../aimbot/aimbot.h"
#include "../armor/armor.h"
#include "../autoaim/tools.h"
#include "geometry_msgs/msg/pose_stamped.hpp"

extern void wheel2land(Target &target, const Pose &pose);

/**
 * @brief 目标姿态解算类
 * 解算出目标装甲板在云台坐标系中的x,y,z坐标
 */
class TargetSolver 
{
 public:
    /// 设置解算模式，计算速度: P3P > EPNP > Iterative, 精确度反之
    constexpr static int ALGORITHM = cv::SOLVEPNP_IPPE;

    /// PNP解算结果旋转矩阵
    cv::Mat rotate_mat;

    /// PNP解算平移矩阵
    cv::Mat trans_mat;

    std::fstream ofs;



 public:
    /**
     * @brief 默认构造函数
     */
    TargetSolver();

    /**
     * @brief 默认析构函数
     */
    ~TargetSolver();

    /**
     * @brief 初始化函数
     *
     * @param file_storage 配置文件
     */
    void init();

    /**
     * @brief 装甲板在相机坐标下解算
     * @param rect 目标装甲板旋转矩形
     * @param is_big_armor 是否是大装甲板, 大小装甲板拥有不同的实际尺寸
     */
   //  void armor_getCoord(const Armor &armor, Target &target);
    bool armor_getCoord(const Armor &armor, const Armor &armor_deputy,Target &target,const Pose &pose, const int& armor_num);
    /**
     * @brief 相机坐标系转换成云台坐标系
     *
     * @param camera_position 输入相机坐标
     * @param ptz_position 输出云台坐标
     */
    void camera2ptz(const cv::Mat &camera_position, Target &ptz_position);

    void projectPoint(Target &target, double std_height);
};

#endif// TARGETSOLVER_HPP
